"""
无2D地图启动:
ros2 launch navigation nav.launch.py
有2D地图启动:
ros2 launch navigation nav.launch.py map_file_path:=maps/6.10lab_mapping/2d_lab_map_-1587_-1537.png
"""
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition
from launch.substitutions import PythonExpression
from launch.actions import ExecuteProcess
import os


def generate_launch_description():
    map_file_path = LaunchConfiguration('map_file_path')
    targets_file_path = LaunchConfiguration('targets_file_path')
    safe_x_range = LaunchConfiguration('safe_x_range')
    nav_param = {
        'map_file_path': map_file_path,
        'targets_file_path': targets_file_path,
        'safe_x_range': safe_x_range,
    }

    publish_2d_map_param = {
        'subscribe_topic': 'Laser_map',
        'publish_topic': 'Laser_2d_map',
        'publish_fram_id': 'camera_init',
        'min_pz': -0.1,
        'max_pz': 0.5,
        'reverse': False,
        'resolution': 0.1,
        'grid_value_threshold': 2.0,  # [2, 3]
        'publish_hz': 1.0 / 3.0
    }

    device = LaunchConfiguration('device')  # 'cuda', 'openvino', 'tensorrt'
    map_adjuster_param = {
        'device': device,
        'delta_angle': 72.0,  # Angle step for alignment in degrees
        'score_threshold': 80  # Minimum number of matches to consider alignment successful
    }

    map_manager_share = get_package_share_directory('map_manager')
    fast_lio2_launch = os.path.join(map_manager_share, 'launch', 'realtime_fast_lio2.launch.py')
    load_2d_map_launch = os.path.join(map_manager_share, 'launch', 'load_2d_map.launch.py')

    navigation_share = get_package_share_directory('navigation')

    return LaunchDescription([
        DeclareLaunchArgument(
            'map_file_path',
            default_value='',
            description='Path to the 2D map file (PNG or YAML). If not provided, the map will not be loaded.'
        ),
        DeclareLaunchArgument(
            'targets_file_path',
            default_value='',
            description='Path to the targets txt file.'
        ),
        DeclareLaunchArgument(
            'safe_x_range',
            default_value='[0.7, 0.4]',
            description='Avoid obstacles in the x range, e.g., [0.7, 0.4].'
        ),
        DeclareLaunchArgument(
            'device',
            default_value='cuda',  # cpu
            description='Inference device for map adjustment: cpu, cuda, openvino, or tensorrt.'
        ),
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(load_2d_map_launch),
            launch_arguments={
                'map_publish_times': '-1',  # Publish map forever, frequency is 1/3 Hz
                'header_frame_id': 'adjusted_init',
                'map_file_path': map_file_path,
                'resolution': '0.1',
                'display_rviz': 'false'
            }.items(),
            condition=IfCondition(PythonExpression([
                '"', map_file_path, '" != ""'
            ]))
        ),
        TimerAction(
            period=5.0,  # 设置延迟启动的时间5秒, 先调整头部角度
            actions=[
                IncludeLaunchDescription(
                    PythonLaunchDescriptionSource(fast_lio2_launch),
                    launch_arguments={
                        'config_file': 'mid360_reverse_no_save.yaml',
                        'rviz_cfg': f'{navigation_share}/config/nav.rviz'
                    }.items()
                ),
            ]
        ),
        Node(
            package='navigation',
            executable='nav_core',
            name='nav_core',
            parameters=[nav_param],
            output='screen'
        ),
        Node(
            package='map_manager',
            executable='publish_2d_map',
            name='publish_2d_map',
            parameters=[publish_2d_map_param],
            output='screen'
        ),
        Node(
            package='navigation',
            executable='map_adjuster.py',
            name='map_adjuster',
            parameters=[map_adjuster_param],
            output='screen'
        )
    ])
